#include<mutex>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/Image.h>
#include<sensor_msgs/image_encodings.h>
#include<sensor_msgs/NavSatFix.h>
#include<ros/ros.h>
#include<thread>
#include<vision_ruler/VisionRuler.h>
#include<vision_ruler/points.h>
#include<vision_ruler/GPS.h>
#include <nav_msgs/Odometry.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>

using namespace std;

string configFilePath;
vision_ruler::VisionRuler visionRuler;
cv::Mat cvImage;
std::mutex imageMut;
std::vector<vision_ruler::box_info> boxes;
int clickNum;
int _x, _y;
double latitude = 0, longitude = 0, altitude = 0;
vision_ruler::global_info ginfo;
ros::Publisher pub;

void cropImage(int event, int x, int y, int flags, void *data) {
    if(cvImage.cols > 0 && cvImage.rows > 0) {
         if(event == cv::EVENT_LBUTTONDOWN) {
             if(clickNum == 0) {
                 _x = x;
                 _y = y;
                 clickNum++;    
             } else {
                 if(_x >= x) {
                     int _x_ = x;
                     x = _x;
                     _x = _x_;
                 }
                 if(_y >= y) {
                     int _y_ = y;
                     y = _y;
                     _y = _y_;
                 }
                 vision_ruler::box_info box;
                 box.left_top_pixel = cv::Point2f(_x, _y);
                 box.right_bottom_pixel = cv::Point2f(x, y);
                 box.boxId = 0;
                 box.comefrom = 1;
                 box.lidar_estimate_distance=10;
                 if(boxes.size() == 0) {
                     boxes.push_back(box);
                 } else {
                     boxes[0] = box;
                 }
                 clickNum = 0;
             }
         } 
     }
}

void doMain() {
    cv::namedWindow("vision-ruler");
    cv::setMouseCallback("vision-ruler", cropImage);
    while(ros::ok()) {
        imageMut.lock();
        if(cvImage.cols > 0 && cvImage.rows > 0) {
            cv::Mat tmpMat = cvImage.clone();
            imageMut.unlock();
            if(boxes.size() > 0) {
                double start = ros::Time::now().toSec();
                visionRuler.computeDistance(boxes, ginfo);
                visionRuler.doRectify = 0;
                double end = ros::Time::now().toSec();
                //ROS_INFO("compute distance cost time %fs", end - start);
                vision_ruler::points vrps;
                for(size_t i = 0; i < boxes.size(); i++) {
                    vision_ruler::box_info box = boxes[i];
                    cv::Rect rect(box.left_top_pixel.x, box.left_top_pixel.y, box.right_bottom_pixel.x - box.left_top_pixel.x, box.right_bottom_pixel.y - box.left_top_pixel.y);
                    cv::rectangle(tmpMat, rect, cv::Scalar(255, 0, 0));
                    int thickness = 3;
                    double fontScale = 2;
                    int baseline = 0;
                    if(box.comefrom==0) {
                        if(!box.isValid) {
                            cv::Size text_size = cv::getTextSize("invalid", cv::FONT_HERSHEY_COMPLEX, fontScale, thickness, &baseline);
                            cv::Point origin(0.5 * (box.right_bottom_pixel.x + box.left_top_pixel.x - text_size.width), 0.5 * (box.right_bottom_pixel.y + box.left_top_pixel.y - text_size.height));
                            cv::putText(tmpMat, "invalid", origin, cv::FONT_HERSHEY_COMPLEX, fontScale, cv::Scalar(0, 255, 255), thickness);
                        } else {
                            std::ostringstream stream;
                            stream << box.distance;
                            cv::Size text_size = cv::getTextSize(stream.str(), cv::FONT_HERSHEY_COMPLEX, fontScale, thickness, &baseline);
                            cv::Point origin(0.5 * (box.right_bottom_pixel.x + box.left_top_pixel.x - text_size.width), 0.5 * (box.right_bottom_pixel.y + box.left_top_pixel.y - text_size.height));
                            cv::putText(tmpMat, stream.str(), origin, cv::FONT_HERSHEY_COMPLEX, fontScale, cv::Scalar(0, 255, 255), thickness);
                        }
                    }
                    vrps.header.stamp = ros::Time::now();
                    vrps.header.frame_id = "vision_ruler";
                    vrps.camera_lat=ginfo.latitude;
                    vrps.camera_lon=ginfo.longitude;
                    vrps.target_lat=box.latitude;
                    vrps.target_lon=box.longitude;
                    pub.publish(vrps);
                }
            }
            cv::imshow("vision-ruler", tmpMat);
            cv::waitKey(30);
        } else {
            imageMut.unlock();
        }
    }
}

void NavSatPOSE_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg) {
    ginfo.latitude = GPS_msg -> latitude;
    ginfo.longitude = GPS_msg -> longitude;
    ginfo.altitude = GPS_msg -> altitude;
    ginfo.camera_gps_degree = 0;
    ginfo.lidar_gps_degree = 0;
}

double getYaw(double x, double y, double z, double w) {
    return atan2(2 * (w * z + x * y), 1 - 2 * (pow(y, 2) + pow(z, 2)));
}

void OdomPOSE_callback(const nav_msgs::OdometryConstPtr &odom_msg) {
    double x = odom_msg->pose.pose.orientation.x;
    double y = odom_msg->pose.pose.orientation.y;
    double z = odom_msg->pose.pose.orientation.z;
    double w = odom_msg->pose.pose.orientation.w;
    double yaw = getYaw(x, y, z, w);
    double degree = yaw * 180.0 / 3.1415926;
    if(degree>=0 && degree<=90) {
        yaw=3.1415926*0.5-yaw;
        degree=yaw * 180.0 / 3.1415926;
    }
    if(degree>90 && degree<=180) {
        yaw=2.5*3.1415926-yaw;
        degree=yaw * 180.0 / 3.1415926;
    }
    if(degree<0) {
        yaw=0.5*3.1415926-yaw;
        degree=yaw * 180.0 / 3.1415926;
    }
    vision_ruler::simple_logger::getInstance()->debug("odom yaw is %.2f", degree);
    ginfo.pose_yaw = degree;
    ginfo.pose_pitch = 0;
}

void GPSPOSE_callback(const vision_ruler::GPSConstPtr &GPS_msg) {
    ginfo.latitude = GPS_msg -> lat;
    ginfo.longitude = GPS_msg -> lon;
    ginfo.altitude = GPS_msg -> alt;
    ginfo.pose_pitch = 0;
    ginfo.camera_gps_degree = 0;
    ginfo.lidar_gps_degree = 0;
    ginfo.pose_yaw = (GPS_msg -> gpsAngle) / 180 * PI;
}

void grabImage(const sensor_msgs::ImageConstPtr& img_msg) {
    cv_bridge::CvImageConstPtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvShare(img_msg);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    imageMut.lock();
    cv_ptr->image.copyTo(cvImage);
    imageMut.unlock();
}

int main(int argc, char **argv) {  
    ros::init(argc, argv, "visonRuler");
    ros::NodeHandle nodeHandler("~");
    
    clickNum = 0;
    string camera_topic, gps_topic, odom_topic;
    nodeHandler.getParam("/domain/configFilePath", configFilePath);
    nodeHandler.getParam("/domain/camera_topic", camera_topic);
    nodeHandler.getParam("/domain/gps_topic", gps_topic);
    nodeHandler.getParam("/domain/odom_topic", odom_topic);

    visionRuler.initFromConfigFile(configFilePath);
    
    int loggerLevel=visionRuler.loggerLevel;
    if(loggerLevel==0) {
        vision_ruler::simple_logger::getInstance()->set_logger_Level(vision_ruler::simple_logger::info_level);
    }
    if(loggerLevel==1) {
        vision_ruler::simple_logger::getInstance()->set_logger_Level(vision_ruler::simple_logger::debug_level);
    }

    pub = nodeHandler.advertise<vision_ruler::points>("/gps_info", 1);
    
    ros::Subscriber sub = nodeHandler.subscribe(camera_topic, 1, &grabImage);
    ros::Subscriber sub2 = nodeHandler.subscribe(gps_topic, 1, &NavSatPOSE_callback);
    ros::Subscriber sub3 = nodeHandler.subscribe(odom_topic, 1, &OdomPOSE_callback);
    //ros::Subscriber sub2 = nodeHandler.subscribe(gps_topic, 1, &NavSatPOSE_callback);

    std::thread th(doMain);
    ros::spin();
    return 0;
}
